/***************************************************************************//**
 * @file camera_array_capture.cpp
 * @brief Contains functionality for capturing images from UVC compliant webcams
 *
 * @page "Camera Capture" Camera Capture
 *
 * @authors Brian Fehrman
 * 
 * @date April 17 2013
 * 
 * @section file_section File Information
 *
 * @details Contains functionality for capturing images from UVC compliant webcams.
 * Can capture up to NxM images simultaenously from an arbitrary camera array
 * matrix configuration. The layout of the cameras and their /dev/video numbers
 * are specified in the settings file. Other information such as capture resolution,
 * output directory, and delay time between image captures can be specified.
 * 
 * 
 * @section settings_section Required Settings
 * 
@verbatim
[base_dir] - name of folder where you want files to be saved (string)
[delay_ms] - delay time in milliseconds between image captures (int)
[width] - width of capture resolution (int)
[height] - height of capture resolution (int)
[cam_layout] - physical layout of cameras and their dev numbers (Mat<int>)
@endverbatim
 * 
 * @section todo_bugs_modification_section Todo, Bugs, and Modifications
 *
 * @todo Add ability to set cameras individually with different settings
 ******************************************************************************/

////////// Includes //////////////
#include "camera_array_capture.h"

/***************************************************************************//**
 * @author Brian Fehrman
 *
 * @par Description:
 * Captures images from an NxM camera matrix simulatenously and then saves a 
 * color version, grayscale version, and an image in which all of the captured
 * images have been combined in accordance with the specified matrix layout. Images
 * are captured at time intervals specified by delay_ms and continues until the user
 * presses a key. A preview of the images are shown and a countdown until the next
 * image capture is also shown.
 *
 * @param[in] fs - FileNode pointing to the settings that are needed
 *
 * @returns 0 - everything went fine
 ******************************************************************************/
int capture_images( FileNode fs )
{  
   string base_dir;
   int delay_ms;
   int width, height;
   Mat cam_layout;
   
   fs["base_dir"] >> base_dir;
   fs["delay_ms"] >> delay_ms;
   fs["width"] >> width;
   fs["height"] >> height;
   fs["cam_layout"] >> cam_layout;
   
   clock_t clk_start;
   float elapsed = 0.0;
   float delay_s = delay_ms / 1000.0;
   int num_cams = cam_layout.rows * cam_layout.cols;
   int counter = 0;
   int num_pic = 0;
   stringstream pix_res;
   VideoCapture cam[ num_cams ];
   vector<string> folder_list;
   string combined_dir;
   vector< int > vid_list;   

   mkdir( base_dir.c_str(), S_IRWXU|S_IRGRP|S_IXGRP );
   base_dir += "/";
   pix_res << width << "_" << height << ".jpg";
   
   combined_dir = base_dir + "combined";
   mkdir( combined_dir.c_str(), S_IRWXU|S_IRGRP|S_IXGRP );
   combined_dir += "/";
   
   //Flatten matrix for easier traversing and create
   //folder for each camera. -1 indicates no cam in that
   //spot
   for( int row = 0; row < cam_layout.rows; row++ )
   {
      for( int col = 0; col < cam_layout.cols; col++ )
      {
         //Don't create folder for cameras designated with -1
         if( cam_layout.at< int >( row, col ) > 0  )
         {
            stringstream cam_folder;
            cam_folder << base_dir <<  "cam_" << row << "_" << col;
            mkdir( cam_folder.str().c_str(), S_IRWXU|S_IRGRP|S_IXGRP );
            
            folder_list.push_back( cam_folder.str() + "/" ); 
         }
         else
         {
            //Add empty string so indexing is easier later
            folder_list.push_back("");
         }
         
         vid_list.push_back( cam_layout.at< int >( row, col ) );
      }
   }
   
   //Open the video feeds and set the resolution, ignore cameras
   //designated with -1
   for( int vid = 0; vid < num_cams; vid++ )
   {
      if( vid_list[ vid ]  > 0 )
      {
         cam[ vid ].open( vid_list[ vid ] );
         
         cam[ vid ].set( CV_CAP_PROP_FRAME_WIDTH, width);
         cam[ vid ].set( CV_CAP_PROP_FRAME_HEIGHT, height );
      }
   }

   namedWindow( "combined_gray", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );
   namedWindow( "combined_color", CV_WINDOW_NORMAL | CV_WINDOW_KEEPRATIO );
   
   clk_start = clock();       
   
   //Capture loop. Goes until user hits a key.
   for( ;; )
   {
      Mat cap_color[ num_cams ];
      Mat cap_gray[ num_cams ];
      Mat combined_gray( height * cam_layout.rows, width * cam_layout.cols, CV_8UC1 );
      Mat combined_color( height * cam_layout.rows, width * cam_layout.cols, CV_8UC3); 
   
      //Capture camera images in parralel and convert to grayscale
      #pragma omp parallel for
      for( int vid = 0; vid < num_cams; vid++)
      {
         cap_gray[ vid ].create( height, width, CV_8UC1 );
         cap_color[ vid ].create( height, width, CV_8UC3 );
         
         if( vid_list[ vid ] > 0 )
         {
            cam[ vid ] >> cap_color[ vid ];
            cvtColor( cap_color[ vid ], cap_gray[ vid ], CV_BGR2GRAY );
         }
      }
      //For each camera in the array
      for( int row_comb = 0; row_comb < cam_layout.rows; row_comb++ )
      {
         for( int col_comb = 0; col_comb < cam_layout.cols; col_comb++ )
         {                   
            //Combine the image feeds in to single displays for the user
            for(int row = 0; row < height; row++ )
            {
               for( int col = 0; col < width; col++ )
               {
                  combined_gray.at<unsigned char>( row + ( height * row_comb ), 
                                          col + ( width * col_comb ) )
                                          = cap_gray[ col_comb + ( row_comb * cam_layout.cols ) ].at<unsigned char>( row, col ); 

                  combined_color.at<Vec3b>( row + ( height * row_comb ), 
                                    col + ( width * col_comb ) )
                                    = cap_color[ col_comb + ( row_comb * cam_layout.cols ) ].at<Vec3b>( row, col );
               }
            }
         }
      }
            
      imshow( "combined_color", combined_color );
      imshow( "combined_gray", combined_gray );
      
      elapsed = ( float( clock() - clk_start ) / CLOCKS_PER_SEC );
      
      //If desired delay time has passed then save the current set of images
      if( elapsed >= delay_s )
      {
         clk_start = clock();
         stringstream ss;
         
         ss << num_pic;
         cout << endl << endl;
         for( int vid = 0; vid < num_cams; vid++ )
         {
            if( vid_list[ vid ]  > 0 )
            {
               string color_save_string = folder_list[ vid ] + ss.str() + "_color_" + pix_res.str();
               string gray_save_string = folder_list[ vid ] + ss.str() + "_gray_" + pix_res.str();
               imwrite( color_save_string, cap_color[ vid ] );
               imwrite( gray_save_string, cap_gray[ vid ] );
            }
         }
         
         imwrite( combined_dir + ss.str() + "_color_" + pix_res.str(), combined_color );
         imwrite( combined_dir + ss.str() + "_gray_" + pix_res.str(), combined_gray );
         
         counter = 0;
         
         cout << "Captured Pic: " << num_pic << endl;
         
         num_pic++;
      }
      
      cout << '\r' << "Time to pic: " << setprecision(2) << fixed << showpoint << ( delay_s - elapsed ) << 's' << flush;
      
      //Waits 30ms and exits loop if user hits key
      if(waitKey(30) >= 0) break;
      
      counter++;
   }
   
   return 0;
}
